//
// Created by yu on 2023/9/27.
//
#include <ros/ros.h>
#include "receiver.h"
#include <serial/serial.h>
#include "msg_node/receiver.h"
using namespace std;

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "receiver");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<msg_node::receiver>("receiver", 1);
  ros::Rate loop_rate(40);
  serial::Serial sp("/dev/receiver", 115200, serial::Timeout::simpleTimeout(100));

  bool imu_open, imu_close = false;

  ROS_INFO("\033[32mReceiver opened successfully...\033[0m");

  while (nh.ok() && sp.isOpen())
  {
    string shit = sp.read(sp.available());

    std::vector<uint8_t> data;

    // 读取数据帧
    if (shit.size() == 35)
    {  // 期望读取35个字节
      for (char c : shit)
      {
        data.push_back(static_cast<uint8_t>(c));
      }
      // 验证帧头
      if (data[0] == 0x0F)
      {
        // 解析中间32字节的16进制数并转换为10进制
        std::vector<uint32_t> decimalValues;
        for (int i = 1; i <= 32; i += 2)
        {
          uint32_t value = (data[i] << 8) | data[i + 1];
          decimalValues.push_back(value);
        }
//        ROS_INFO("1: %d,2: %d,3: %d,4: %d,5: %d,6: %d,7: %d,8: %d",
//                 decimalValues[0],
//                 decimalValues[1],
//                 decimalValues[2],
//                 decimalValues[3],
//                 decimalValues[4],
//                 decimalValues[5],
//                 decimalValues[6],
//                 decimalValues[7]);
        msg_node::receiver _recData;
        //通道1，副翼
        _recData.receiver1 = float(decimalValues[0] - 385) / 1278;
        //通道2，升降
        _recData.receiver2 = float(decimalValues[1] - 385) / 1278;
        //通道3，油门
        _recData.receiver3 = float(decimalValues[2] - 385) / 1278;
        //通道4，方向
        _recData.receiver4 = float(decimalValues[3] - 385) / 1278;
        //通道5，S3
        _recData.receiver5 = float(decimalValues[4] - 385) / 1278;
        //通道6，S4
        _recData.receiver6 = float(decimalValues[5] - 385) / 1278;
        //通道7，S5
        _recData.receiver7 = float(decimalValues[6] - 385) / 1278;
        //通道8，S6
        _recData.receiver8 = float(decimalValues[7] - 385) / 1278;
        pub.publish(_recData);
//
//      std::vector<std::string> nodes;
//      ros::master::getNodes(nodes);
//      if (decimalValues[4] > 1200 && !imu_open)
//      {
//        system("gnome-terminal -- bash -c 'roslaunch imu rviz_and_imu.launch'");
//        imu_open = true;
//      }
//      else if (decimalValues[4] < 1200 && std::binary_search(nodes.begin(), nodes.end(), "/imu"))
//      {
//        system("rosnode kill imu");
//        system("rosnode kill rviz");
//        imu_open = false;
//      }


      }

    }
    loop_rate.sleep();
  }

}
